#include "xen/physical/world.hpp"

#include "xen/physical/body.hpp"

namespace Xen::Physical {

        World::World() {
            id_ = dWorldCreate();
        }

        World::~World() {
            dWorldDestroy(id_);
        }

        Ref<SphereBody> World::CreateSphere(float mass, const glm::vec3& position, float r) {
            return std::make_shared<SphereBody>(*this, mass, position, r);
        }

        Ref<BoxBody> World::CreateBox(float mass, const glm::vec3 &position, const glm::vec3& l) {
            return std::make_shared<BoxBody>(*this, mass, position, l);
        }

        void World::SetGravity(const glm::vec3& g) {
            dWorldSetGravity(id_, g.x, g.y, g.z);
        }

        const glm::vec3 World::GetGravity() const {
            dReal g[3];
            dWorldGetGravity(id_, g);
            return glm::vec3(g[0], g[1], g[2]);
        }

        void World::SetERP(float value) {
            value = Clamp(0.0f, 1.0f, value);
            dWorldSetERP(id_, value);
        }

        void World::SetCFM(float value) {
            value = value < 0 ? 0 : value;
            dWorldSetCFM(id_, value);
        }

        void World::Update(TimeStep& ts) {
            dWorldStep(id_, ts.GetSecondTime());
        }

}
